
/*
* core one, cooperator.
* proc message.
*/

#include <stdint.h>
#include <stdio.h>

#include "rp2040.h"
#include "pico.h"
#include "pico/multicore.h"

#include "msgpool.h"
#include "umsg.h"

#include "hardware/i2c.h"

/*
* 0 = data[0],  init : [1] [2] [3] [4] = baud
* 1 = data[0],  read : [1] = addr, [2] = cnt
* 2 = data[0], write : [1] = addr, [...] = data
* 3 = data[0], general call : write..
*/
void p1_test_i2c( umsg_t * pmsg )
{
	i2c_inst_t * pi2c = i2c0;
	uint8_t * ptr = pmsg->data;
	uint32_t temp;
	uint8_t addr;
	int tlen;
	int iret;
	int i;
	uint8_t tary[32];
	uint8_t repy[4];


	switch ( ptr[0] ) {
	
	case 0:
		temp = ptr[4];
		temp = (temp << 8) | ptr[3];
		temp = (temp << 8) | ptr[2];
		temp = (temp << 8) | ptr[1];
		printf("i2c init, %u\n", temp );
		i2c_init( pi2c, temp );
		break;

	case 1:
		addr = ptr[1];
		tlen = ptr[2];
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );

		printf("read = %d\n", iret );
		
		if ( iret > 0 ) {
			int  i;
			for ( i=0; i<iret; i++ ) {
				printf( "%x : ", tary[i] );
			}
			printf( "\n\n");
		}

		break;

	case 2:
		addr = ptr[1];
		tlen = pmsg->mlen - 2;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 2];
		}

		iret = i2c_write_blocking( pi2c, addr, tary, tlen, false );
		if ( iret != tlen ) {
			printf( "write, ret = %d\n", iret );
			break;
		}

		printf( "write, succ\n" );
		break;

	case 3:
		/* process call, write, restart, read */
		addr = ptr[1];
		tlen = pmsg->mlen - 3;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 3];
		}

		repy[0] = (uint8_t)tlen;
		iret = i2c_write_blocking( pi2c, addr, tary, tlen, true );
		if ( iret != tlen ) {
			printf( "write, ret = %d\n", iret );
			break;
		}

		tlen = ptr[2];
		repy[1] = (uint8_t)tlen;
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );
		if ( iret != tlen ) {
			printf( "read, ret = %d\n", iret );
			break;
		}

		repy[2] = 0x55;
		repy[3] = 0x66;
		break;
	
	case 4:
		/* StartByte, master send  */

		addr = ptr[1];
		tlen = pmsg->mlen - 2;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 2];
		}

		pi2c->hw->enable = 0;
		temp = I2C_IC_TAR_SPECIAL_VALUE_ENABLED << I2C_IC_TAR_SPECIAL_LSB;
		temp = temp | (I2C_IC_TAR_GC_OR_START_VALUE_START_BYTE << I2C_IC_TAR_GC_OR_START_LSB);
		temp = temp | addr;
		pi2c->hw->tar = temp;
		pi2c->hw->enable = 1;

		for ( i=0; i<tlen; i++ ) {

			temp = I2C_IC_DATA_CMD_CMD_VALUE_WRITE << I2C_IC_DATA_CMD_CMD_LSB;
			temp = temp | tary[i];

			if ( (i + 1) == tlen ) {
				temp = temp | I2C_IC_DATA_CMD_STOP_VALUE_ENABLE << I2C_IC_DATA_CMD_STOP_LSB;
			}
			
			pi2c->hw->data_cmd = temp;
		}

		/* check and wait */
		while ( true ) {

			temp = pi2c->hw->raw_intr_stat;

            if ( temp & I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS ) {
            	iret = 0x11;
                break;
            }

            if ( temp & I2C_IC_RAW_INTR_STAT_STOP_DET_BITS ) {
            	iret = 0;
                break;
            }

#if 0
            if ( (pi2c->hw->status & 0x1) == 0 ) {
            	iret = 0;
                break;
            }
#endif

		}

        temp = pi2c->hw->tx_abrt_source;
        if ( temp != 0 ) {
            // Note clearing the abort flag also clears the reason, and
            // this instance of flag is clear-on-read! Note also the
            // IC_CLR_TX_ABRT register always reads as 0.
            pi2c->hw->clr_tx_abrt;
            pi2c->hw->clr_intr;
		}
        
        repy[0] = temp & 0xFF;
        repy[1] = (temp >> 8) & 0xFF;
        repy[2] = (temp >> 16) & 0xFF;
        repy[3] = (temp >> 24) & 0xFF;

        /**/
        pi2c->hw->enable = 0;
        break;

	default:
		iret = -1;
		break;
	}

	pmsg->type = 0x10;
	pmsg->mlen = 5;
	pmsg->data[0] = (uint8_t)iret;
	pmsg->data[1] = repy[0];
	pmsg->data[2] = repy[1];
	pmsg->data[3] = repy[2];
	pmsg->data[4] = repy[3];
	multicore_fifo_push_blocking( (intptr_t)pmsg );
	return;
}


void core1_main( void )
{
	umsg_t * pmsg;
	uint32_t cnt = 0;

	printf("hello, from, %d\n", sio_hw->cpuid );
	
	while ( 1 ) {

		/**/
		pmsg = (umsg_t *)(uintptr_t)multicore_fifo_pop_blocking();
		cnt += 1;

		// printf( "pmsg, %u: %p, %u, %u\n", cnt, pmsg, pmsg->type, pmsg->mlen );
		
		switch ( pmsg->type ) {

		case 0x7F:
			multicore_fifo_push_blocking( (intptr_t)pmsg );
			break;
		
		case 0x10:
			p1_test_i2c( pmsg );
			break;

		default:
			msg_free( pmsg );
			break;
		}

		// test, echo..
		// multicore_fifo_push_blocking( (uint32_t)(uintptr_t)pmsg );
	}
}


